#include "./pick_motor.h"
#include "../xlib/gpio.h"
#include "../xlib/pwm.h"
#include "../rtos/event_flags.h"
#include "../pub_driver/step_motor_drv.h"
#include "./sensor.h"

namespace cargo_driver {

PickMotor pickMotor;

namespace pick_motor_impl {

using namespace stm32f1;
using namespace rtos::lazy;
using namespace pub_driver;

using PULS = gpio::PA<1>;
using DIR = gpio::PC<3>;
using ENABLE = gpio::PC<5>;
using ALM = gpio::PC<4>;
using PWM = pwm::Timer2PWM<2>;

struct PickDrvCfg {

    using PULS_PIN = PULS;
    
    static constexpr uint32_t RESET_PULS_COUNT = 400;
    static constexpr uint32_t SLOW_DELAY_MS = 2;
    static constexpr uint8_t EC_ALM = ec::cargo::PICK_ALM;
    static constexpr uint8_t EC_TIMEOUT = ec::cargo::PICK_TIMEOUT;

    static constexpr uint32_t BASE_FREQ = 200;
    static constexpr uint32_t ACC = 4;
    static constexpr uint32_t VAR_RANGE = 600;

    static constexpr uint32_t RESET_FREQ = 400;
    static constexpr uint32_t RESET_TIMEOUT = 30 * 1000;

    static void set_dir(SimpleDir dir) {
        switch (dir) {
            case SimpleDir::ENTER: DIR::clr(); break;
            case SimpleDir::EXIT: DIR::set(); break;
            case SimpleDir::DIRECT: break;
        }
    }

    static bool is_origin() {
        return sensor.is_pick_motor_origin();
    }

    static bool is_alm() {
        return !ALM::read();
    }

    static void enable_pwm(uint32_t freq) {
        PULS::set_mode<gpio::Mode::OUT_50MHz_AF_PP>();
        PWM::start_with_irq(freq);
    }

    static void enable_pwm_not_irq(uint32_t freq) {
        PULS::set_mode<gpio::Mode::OUT_50MHz_AF_PP>();
        PWM::start_with_no_irq(freq);
    }

    static void disable_pwm() {
        PWM::stop();
        PULS::set_mode<gpio::Mode::OUT_50MHz_PP>();
    }

    static void set_pwm_freq(uint32_t freq) {
        PWM::set_freq(freq);
    }

};

static StepMotorDrv<PickDrvCfg> drv;

}

using namespace pick_motor_impl;

void PickMotor::init() {

    drv.init();

    rcc::enable<
        PULS, DIR, ENABLE, ALM, PWM
    >();

    gpio::config<
        gpio::Cfg<PULS, gpio::Mode::OUT_50MHz_PP>,
        gpio::Cfg<DIR, gpio::Mode::OUT_50MHz_PP>,
        gpio::Cfg<ENABLE, gpio::Mode::OUT_50MHz_PP>,
        gpio::Cfg<ALM, gpio::Mode::INPUT_FLOAT>
    >();
    ENABLE::clr();
    PWM::stop();

    NVIC_EnableIRQ(TIM2_IRQn);
}

Error PickMotor::async_run(uint32_t position) {
    return drv.async_run_target(position);
}

Error PickMotor::wait_for() {
    return drv.wait_for();
}

bool PickMotor::is_alm() const {
    return PickDrvCfg::is_alm();
}

}

using namespace cargo_driver;

extern "C"
void TIM2_IRQHandler() {
    if (PWM::is_uie()) {
        PWM::clear_flag();
        drv.on_puls();
    }
}


